#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <cv_eyehand/TutorialsConfig.h>
#include "arm_controller/move.h"

class move_arm
{
public:
	move_arm();
	bool goto_pos(double x,double y,double z);
	void callback(cv_eyehand::TutorialsConfig &config, uint32_t level);
private:
	ros::NodeHandle n;
	ros::ServiceClient goto_client;
};

move_arm::move_arm(){
	goto_client = n.serviceClient<arm_controller::move>("goto_position"); //定义"goto_position"客户端
	dynamic_reconfigure::Server<cv_eyehand::TutorialsConfig> server;    
	dynamic_reconfigure::Server<cv_eyehand::TutorialsConfig>::CallbackType f;    
	//并将回调函数和服务端绑定
	f = boost::bind(&move_arm::callback,this, _1, _2);    
	server.setCallback(f);    
	ROS_INFO("Spinning node");    
	ros::spin();
}

bool move_arm::goto_pos(double x,double y,double z){
    ros::service::waitForService("goto_position"); //等待“goto_position”服务开启
    arm_controller::move srv; // 创建服务消息
    srv.request.pose.position.x = x;
    srv.request.pose.position.y = y;
    srv.request.pose.position.z = z;
    goto_client.call(srv); //发送请求
    return srv.response.success;
}

//定义回调函数，当客户端请求修改参数时，服务端即可跳转到回调函数中进行处理。
//传入值有两个，一个是参数更新的配置值，一个是表示参数修改的掩码
void move_arm::callback(cv_eyehand::TutorialsConfig &config, uint32_t level) 
{
    goto_pos(config.position_x,config.position_y,config.position_z);
	ROS_INFO("Reconfigure Request: %d %d %d",
			config.position_x,
			config.position_y,
			config.position_z);
}

int main(int argc, char **argv)
{    
	//初始化ROS节点
	ros::init(argc, argv, "move_arm");
	move_arm mv;
	return 0;
}
